/**
 * 
 */

package tools;

import interfaces.IGetSmoothing;

/**
 * @author Vivek
 *
 */
public class CalculateSmoothing implements IGetSmoothing {

	/**
	 * This method calculates the angles between given set of points 
	 * 
	 * 
	 * 1)	To calculate squared angle cost. We need at-least 3 points as we are calculating angle between sequences of paths. These paths are assumes to be line segments.
	 * 2)	Angle between points are calculated as below:
	 * 		a.	First we calculate slope of each line segment:
	 * 		b.	Then we calculate angle between both the gradients:
	 * 3) Angle for more than 3 points can be calculated in which case the function just calculates the sum of angles. 
	 * 
	 * Assumptions:
	 * Minimum number of points are 3. Exception yet to be implemented..  
	 * 
	 * @param xy
	 * @return
	 */
	public double[] getAngle(double[][] xy) {
		double slope1 = 0;
		double slope2 = 0;
		double angles[] = new double[xy.length-2]; 
		
		for(int n=2; n < xy.length;n++){	
			slope1 = (xy[n-1][1] - xy[n-2][1])/(xy[n-1][0] - xy[n-2][0]);
			slope2 = (xy[n][1] - xy[n-1][1])/(xy[n][0] - xy[n-1][0]);			
			angles[n-2]= (Math.atan((slope2-slope1)/(1+slope1*slope2)));
		}
		return angles;
	}
	

	/**
	 * This method calculates the distance between given set of points 
	 * 
	 * 
	 * Assumptions:
	 * Minimum number of points are 2. Exception yet to be implemented..  
	 * 
	 * @param xy
	 * @return
	 */
	public double[] getLength(double[][] xy) {
		double length[] = new double[xy.length-2]; 
		
		for(int n=2; n < xy.length;n++){	
			length[n-2] =  Math.sqrt(Math.pow((xy[n-1][0] - xy[n][0]),2) + Math.pow((xy[n-1][1] - xy[n][1]),2));

		}
		return length;
	}

	/*  
	 * (non-Javadoc)
	 * @see interfaces.IGetSmoothing#getSmoothnessValue(double[][])
	 */
	@Override
	public double getSmoothnessValue(double[][] xy) {
		
		double length = 0;
		double slope1 = 0;
		double slope2 = 0;
		double passValues=0;
		
		for(int n=2; n < xy.length;n++){
			
			slope1 = (xy[n-1][1] - xy[n-2][1])/(xy[n-1][0] - xy[n-2][0]);
			slope2 = (xy[n][1] - xy[n-1][1])/(xy[n][0] - xy[n-1][0]);

			length =  Math.sqrt(Math.pow((xy[n-2][0] - xy[n-1][0]),2) + Math.pow((xy[n-2][1] - xy[n-1][1]),2));
			
//			System.out.println("Length:"+length);
//			passValues += Math.pow(Math.toDegrees(Math.atan((slope2-slope1)/(1+slope1*slope2))),2);//*length;
			
			passValues += Math.pow((Math.atan((slope2-slope1)/(1+slope1*slope2))),2)*length;
			
//			System.out.println("Angle ="+Math.atan((slope2-slope1)/(1+slope1*slope2)));
//			System.out.println("Angle ="+Math.pow(Math.atan((slope2-slope1)/(1+slope1*slope2)),2));
			
//			System.out.println(xy[n-1][1]+"- "+xy[n-2][1]+" / "+xy[n-1][0]+" - "+xy[n-2][0]);
//			System.out.println(xy[n][1]+"- "+xy[n-1][1]+" / "+xy[n][0]+" - "+xy[n-1][0]);
//			System.out.println((slope2-slope1)/(1+slope1*slope2));
//			System.out.println(" Length="+length+" Degree="+degree+" Pow="+passValues);
		}
		
//		System.out.println("Length="+length);
		
		return passValues;
	
	}
	@Override
	public String getAlgorithmName() {
		return "Squared angle method";
	}

}
